#include "device/qmc5883p/qmc5883.h"

uint8_t QMC5883P::SetGain()
{
    switch (Gain)
    {
    case R30G:
        /* code */
        return 0;
        //break;
    case R12G:
        return 1;
       // break;
    case R8G:
        return 2;
        //break;
    case R2G:
        return 3;
        //break;
    default:
    return 0;
        //break;
    }
}

uint16_t QMC5883P::getSensitivity()
{
     switch (Gain)
    {
    case R30G:
        /* code */
        return 1000;
        //break;
    case R12G:
        return 2500;
        //break;
    case R8G:
        return 3750;
        //break;
    case R2G:
        return 15000;
        //break;
    default:
        return 1000;
        //break;
    }
}

uint8_t QMC5883P::SetMode()
{
    switch (mode)
    {
    case Single:
        /* code */
        return 2;
        //break;
    case Suspend:
        return 0;
        //break;
    case Normal:
        return 1;
        //break;
    case Continuous:
        return 3;
        //break;
    default:
        return 3;
        //break;
    }
}
uint8_t QMC5883P::SetODR()
{
    switch (odr)
    {
    case ODR_10:
        /* code */
        return 0;
        //break;
    case ODR_50:
        return 1;
        //break;
    case ODR_100:
        return 2;
        //break;
    case ODR_200:
        return 3;
        //break;
    default:
        return 3;
        //break;
    }
}
uint8_t QMC5883P::SetFilter()
{
    switch (filter)
    {
    case OSR_8:
        /* code */
        return 0;
        //break;
    case OSR_4:
        return 1;
        //break;
    case OSR_2:
        return 2;
        //break;
    case OSR_1:
        return 3;
        //break;
    default:
        return 0;
       // break;
    }
}
uint8_t QMC5883P::SetDepth()
{
    switch (depth)
    {
    case OSR_2_1:
        /* code */
        return 0;
        //break;
    case OSR_2_2:
        return 1;
        //break;
    case OSR_2_4:
        return 2;
        //break;
    case OSR_2_8:
        return 3;
        //break;
    default:
        return 3;
        //break;
    }
}

uint8_t QMC5883P::getControlReg()
{
    uint8_t reg = 0;
    reg = SetMode() | (SetODR() << 2) | (SetFilter() << 4) | (SetDepth() << 6);
    return reg;
}

uint8_t QMC5883P::getControlReg2()
{
    uint8_t reg = 0;
    reg = SetSetRestMode() | (SetGain() << 2) | ((SetSelfTest(0) | (SoftReset(0) << 1) )<< 6);
    return reg;
}

uint8_t QMC5883P::SetSetRestMode()
{
     switch (setMode)
    {
    case SET_RESET_ON:
        /* code */
        return 0;
        //break;
    case SET_ONLY_ON:
        return 1;
        //break;
    case SET_RESET_OFF:
        return 2;
        //break;
    default:
        return 0;
        //break;
    }
}

void QMC5883P::SetUp()
{
    uint8_t count = 0;
    uint8_t data;
    while (!TestConnection())
    {
        count++;
        if (count > 50)
            return;
    }
    SoftReset();
    delay_ms(5);

    /*switch (mode)
    {
    case Suspend:
        data[0] = 0x00;
        i2c_write(ADDR, MODE_REG, 1, data);
        break;
    case Normal:
        data[0] = 0x06; // define x,y z
        data[1] = 0x08;
        data[2] = 0xCD;
        i2c_write(ADDR, DEFINE_REG, 1, &data[0]);
        i2c_write(ADDR, SELT_TEST_REG, 1, &data[1]);
        i2c_write(ADDR, MODE_REG, 1, &data[2]);
        break;
    case Continuous:
        data[0] = 0x06; // define x,y z
        data[1] = 0x08;
        data[2] = 0xC3;
        i2c_write(ADDR, DEFINE_REG, 1, &data[0]);
        i2c_write(ADDR, SELT_TEST_REG, 1, &data[1]);
        i2c_write(ADDR, MODE_REG, 1, &data[2]);

        break;

    default:
        break;
    }*/
		
//		data = 0x00;
//		i2c_write(ADDR,0x0B,1,&data);
//		delay_ms(10);
		
		/*data = 0x40;
		i2c_write(ADDR,0x0d,1,&data);
		delay_ms(10);
		
		data = 0x06;
		i2c_write(ADDR,0x29,1,&data);
		
		delay_ms(10);
		
		data = 0x04;
		i2c_write(ADDR,0x0B,1,&data);
		delay_ms(10);
		
		
		
		data = 0xCF;
		i2c_write(ADDR,0x0A,1,&data);
		
		delay_ms(10);
		
		data = 0;
		i2c_read(ADDR,0x0B,1,&data);
		data = 1;*/
		
//    data = 0x06;
//    i2c_write(ADDR,DEFINE_REG,1,&data);
//		
//		data = getControlReg2();
//    i2c_write(ADDR,SELT_TEST_REG,1,&data);
//		
//    data = getControlReg();
//    i2c_write(ADDR,MODE_REG,1,&data);
//		
//		data = 0;
//		
//		i2c_read(ADDR,MODE_REG,1,&data);
//		data = 0;
		//i2c_read(ADDR,MODE_REG,1,&data);
		//SoftReset();
		//data = 0;
		//i2c_read(ADDR,MODE_REG,1,&data);

        data = 0x00;
        i2c_write(ADDR,SELT_TEST_REG,1,&data);
        data = 0x06;
        i2c_write(ADDR,DEFINE_REG,1,&data);

        data = getControlReg2();
        i2c_write(ADDR,SELT_TEST_REG,1,&data);

        data = getControlReg();
        i2c_write(ADDR,MODE_REG,1,&data);
}

void QMC5883P::SoftReset()
{
    uint8_t data = 0x80;
    i2c_write(ADDR, SELT_TEST_REG, 1, &data);
}

uint8_t QMC5883P::TestConnection()
{
    uint8_t result = 0;
    i2c_read(ADDR, CHIP_ID, 1, &result);
    return (result == 0x80) ? 1 : 0;
}

void QMC5883P::SelfTest()
{
    uint8_t data = 0;
    uint8_t data1_xyz[6];
    uint8_t data2_xyz[6];
    int16_t data1[3];
    int16_t data2[3];
    uint8_t count = 0;
    data = 0x06;
    i2c_write(ADDR, DEFINE_REG, 1, &data);
    data = 0x03;
    i2c_write(ADDR, MODE_REG, 1, &data);
    do
    {
        i2c_read(ADDR, STATUS, 1, &data);
        count++;
        if (count >= 10)
            break;
    } while ((data & 0x01) == 1);
    i2c_read(ADDR, X_L_OUT, 6, data1_xyz);
    data1[0] = (int16_t)(data1_xyz[0] | ((uint16_t)data1_xyz[1] << 8));
    data1[1] = (int16_t)(data1_xyz[2] | ((uint16_t)data1_xyz[3] << 8));
    data1[2] = (int16_t)(data1_xyz[4] | ((uint16_t)data1_xyz[5] << 8));
    data = 0x40;
    i2c_write(ADDR, SELT_TEST_REG, 1, &data);
		i2c_read(ADDR, SELT_TEST_REG, 1, &data);
    delay_ms(1000);
    i2c_read(ADDR, X_L_OUT, 6, data2_xyz);
    data2[0] = (int16_t)(data2_xyz[0] | ((uint16_t)data2_xyz[1] << 8));
    data2[1] = (int16_t)(data2_xyz[2] | ((uint16_t)data2_xyz[3] << 8));
    data2[2] = (int16_t)(data2_xyz[4] | ((uint16_t)data2_xyz[5] << 8));
    delta_xyz[0] = (float)(data2[0] - data1[0]);
    delta_xyz[1] = (float)(data2[1] - data1[1]);
    delta_xyz[2] = (float)(data2[2] - data1[2]);

    float theory = 1.1f * 1000.0f;
    factor[0] = theory / abs(delta_xyz[0]);
    factor[1] = theory / abs(delta_xyz[1]);
    factor[2] = theory / abs(delta_xyz[2]); 
		//i2c_read(ADDR, SELT_TEST_REG, 1, &data);
}

void QMC5883P::GetMeasureMentData()
{
    uint8_t data = 0;
    uint8_t raw_data[6];
    i2c_read(ADDR, STATUS, 1, &data);
		
    if ((data & 0x01))
    {
			  i2c_read(ADDR,SELT_TEST_REG,1,&data);
        i2c_read(ADDR, X_L_OUT, 6, raw_data);
        rawData_t.mx_raw = (int16_t)(raw_data[0] | ((uint16_t)raw_data[1] << 8));
        rawData_t.my_raw =  (int16_t)(raw_data[2] | ((uint16_t)raw_data[3] << 8));
        rawData_t.mz_raw =  (int16_t)(raw_data[4] | ((uint16_t)raw_data[5] << 8));
        rawData_t.mx = rawData_t.mx_raw * factor[0] / (float)getSensitivity();
        rawData_t.my = rawData_t.my_raw * factor[1] / (float)getSensitivity();
        rawData_t.mz = rawData_t.mz_raw * factor[2] / (float)getSensitivity();
    }
}
